#pragma once
#include <rclcpp/rclcpp.hpp>
#include <tf2_msgs/msg/tf_message.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

class TagSaveMap : public rclcpp::Node
{
public:
public:
  TagSaveMap() : Node("tag_save_map")
  {
    // 初始化TF2缓冲区和监听器
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    // 创建定时器定期查询变换
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(100),
        std::bind(&TagSaveMap::TFCallback, this));
    
        // 订阅动态和静态TF话题
    tf_sub_ = create_subscription<tf2_msgs::msg::TFMessage>(
      "/tf", 10,
      [this](const tf2_msgs::msg::TFMessage::SharedPtr msg) {
          processTF(msg->transforms);
      });
  
  tf_static_sub_ = create_subscription<tf2_msgs::msg::TFMessage>(
      "/tf_static", 10,
      [this](const tf2_msgs::msg::TFMessage::SharedPtr msg) {
          processTF(msg->transforms);
      });
  }

  void TFCallback();
  void processTF(const std::vector<geometry_msgs::msg::TransformStamped>& transforms);
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  rclcpp::TimerBase::SharedPtr timer_;

  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_;
  rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_static_sub_;

  std::set<std::string> tag_frames_;
  std::mutex mutex_;
};